﻿package rob.maths{
	import rob.data.Matrix4;
	
	public class MatrixMaths{
		
		public function MatrixMaths():void{
		}
		
		// only consider 3x3 section e.g. basis rotate
		public static function vectMatMult3x3(m:Matrix4, v):void{
			var vx:Number, vy:Number, vz:Number;
			v.x = m.m11 * (vx=v.x) + m.m12 * (vy=v.y) + m.m13 * (vz=v.z);
			v.y = m.m21 * vx + m.m22 * vy + m.m23 * vz;
			v.z = m.m31 * vx + m.m32 * vy + m.m33 * vz;
		}
		
		// 4x4 axis rotation, acts as identity on 4th, outputs matrix
		// angle in radians
		public static function axisRot(axis, angle:Number):Matrix4{
			var u:Number = axis.x, v:Number = axis.y, w:Number = axis.z;
			var m:Matrix4 = Matrix4.createId();
			var sa:Number = Math.sin(angle);
			var ca:Number = Math.cos(angle);
			var cf:Number = 1 - ca;
			
			var suv:Number = u * v * cf;
			var svw:Number = v * w * cf;
			var suw:Number = u * w * cf;
			var sw:Number = sa * w ;
			var sv:Number = sa * v ;
			var su:Number = sa * u ;
			
			m.m11  =   ca + u * u * cf;
			m.m12  = - sw 	+ suv;
			m.m13  =   sv 	+ suw;

			m.m21  =   sw 	+ suv;
			m.m22  =   ca + v * v * cf;
			m.m23  = - su 	+ svw;

			m.m31  = - sv	+ suw;
			m.m32  =   su	+ svw;
			m.m33  =   ca	+ w * w * cf;

			return m;
		}
	}
}